#include "iopad.h"
#include "pinctrl.h"
#include <asm/arch/hardware.h>
// #define  FH_PINCTRL_DEBUG
#ifdef FH_PINCTRL_DEBUG
#define PRINT_DBG(fmt,args...)  OS_PRINT(fmt,##args)
#else
#define PRINT_DBG(fmt,args...)  do{} while(0)
#endif

static PinCtrl_Object pinctrl_obj;
OS_LIST fh_pinctrl_devices = OS_LIST_INIT(fh_pinctrl_devices);

static int fh_pinctrl_func_select(PinCtrl_Pin *pin, unsigned int flag)
{
    unsigned int reg;

    if(!pin)
    {
        OS_PRINT("ERROR: pin is null\n\n");
        return -1;
    }

    if(flag & NEED_CHECK_PINLIST)
    {
        if(pinctrl_obj.pinlist[pin->pad_id])
        {
            OS_PRINT("ERROR: pad %d has already been set\n\n", pin->pad_id);
            return -2;
        }
    }

    reg = GET_REG(pinctrl_obj.vbase + pin->reg_offset);

    pin->reg = (PinCtrl_Register *)&reg;

    pin->reg->bit.mfs = pin->func_sel;

    pin->reg->bit.pdn = 0;
    pin->reg->bit.pun = 0;

    if(pin->pullup_pulldown == PUPD_DOWN)
    {
	    pin->reg->bit.pdn = 1;
    }
    else if(pin->pullup_pulldown == PUPD_UP)
    {
	    pin->reg->bit.pun = 1;
    }
    else
    {
	    pin->reg->bit.pdn = 0;
	    pin->reg->bit.pun = 0;
    }

    pin->reg->bit.ds = pin->driving_curr;
    pin->reg->bit.st = 1;

    pin->reg->bit.ie = 1;
    pin->reg->bit.oe = pin->output_enable;

    SET_REG(pinctrl_obj.vbase + pin->reg_offset, pin->reg->dw);

    pinctrl_obj.pinlist[pin->pad_id] = pin;

    return 0;
}

static int fh_pinctrl_mux_switch(PinCtrl_Mux *mux, unsigned int flag)
{
    if(mux->cur_pin > mux->mux_num)
    {
        OS_PRINT("ERROR: selected function is not exist, sel_func=%d\n\n", mux->cur_pin);
        return -3;
    }

    if(!mux->mux_pin[mux->cur_pin])
    {
        OS_PRINT("ERROR: mux->mux_pin[%d] has no pin\n\n", mux->cur_pin);
        return -4;
    }

    PRINT_DBG("\t%s[%d]\n", mux->mux_pin[mux->cur_pin]->func_name, mux->cur_pin);
    return fh_pinctrl_func_select(mux->mux_pin[mux->cur_pin], flag);
}


static int fh_pinctrl_device_switch(PinCtrl_Device *dev, unsigned int flag)
{
    int i, ret;
    for(i=0; i<dev->mux_count; i++)
    {
        unsigned int *mux_addr = (unsigned int *)((unsigned int)dev
                + sizeof(*dev) - 4 + i*4);
        PinCtrl_Mux *mux = (PinCtrl_Mux *)(*mux_addr);

        ret = fh_pinctrl_mux_switch(mux, flag);
        if(ret)
        {
            return ret;
        }
    }

    if (strncmp(dev->dev_name, "SD1", 3) == 0)
    {
        PinCtrl_Mux *mux = (PinCtrl_Mux *)dev->mux;

        /* for fh865x, set SD1_FUNC_SEL after all mux switch */
        SET_REG(REG_PMU_SD1_FUNC_SEL, mux->cur_pin);
    }

    return 0;
}

static PinCtrl_Device * fh_pinctrl_get_device_by_name(char *name)
{
    PinCtrl_Device *dev = OS_NULL;

    list_for_each_entry(dev, &fh_pinctrl_devices, list)
    {
        if(!strcmp(name, dev->dev_name))
        {
            return dev;
        }
    }

    return 0;
}

static PinCtrl_Mux *fh_pinctrl_get_mux_by_name(char *mux_name)
{
    int i;
    PinCtrl_Device *dev;

    list_for_each_entry(dev, &fh_pinctrl_devices, list) {
        for (i = 0; i < dev->mux_count; i++) {
            unsigned int *mux_addr = (unsigned int *)(
                (unsigned int)dev + sizeof(*dev) - 4 + i*4);
            PinCtrl_Mux *mux = (PinCtrl_Mux *)(*mux_addr);

            if (!strcmp(mux_name, mux->mux_pin[0]->func_name))
                return mux;
        }
    }

    return NULL;
}

static PinCtrl_Pin *fh_pinctrl_get_pin_by_name(char *pin_name)
{
    int i;
    PinCtrl_Pin *pin = NULL;

    for (i = 0; i < PAD_NUM; i++) {
        pin = pinctrl_obj.pinlist[i];
        if (!pin || !pin->func_name)
            continue;
        if (!strcmp(pin->func_name, pin_name))
            return pin;
    }

    return NULL;
}


int fh_pinctrl_check_pinlist(void)
{
    int i;
    for(i=0; i<PAD_NUM; i++)
    {
        if(!pinctrl_obj.pinlist[i])
        {
            OS_PRINT("ERROR: pad %d is still empty\n", i);
        }
    }

    return 0;
}

static int fh_pinctrl_init_devices(char** devlist, int listsize, unsigned int flag)
{
    int i, ret;
    PinCtrl_Device *dev;

    memset(pinctrl_obj.pinlist, 0, sizeof(pinctrl_obj.pinlist));

    for(i=0; i<listsize; i++)
    {
        dev = fh_pinctrl_get_device_by_name(devlist[i]);

        if(!dev)
        {
            OS_PRINT("ERROR: cannot find device %s\n", devlist[i]);
            return -5;
        }

        PRINT_DBG("%s:\n", dev->dev_name);
        ret = fh_pinctrl_device_switch(dev, flag);
        PRINT_DBG("\n");
        if(ret)
        {
            return ret;
        }

    }

    if(flag & NEED_CHECK_PINLIST)
        fh_pinctrl_check_pinlist();

    return 0;

}

static void fh_pinctrl_init_pin(void)
{
    int i;

    for(i=0; i<PAD_NUM; i++)
    {
        PinCtrl_Pin *pin = pinctrl_obj.pinlist[i];
        if (!pin)
        {
            PinCtrl_Register reg;

//            PRINT_DBG("ERROR: pad %d is empty\n", i);
            reg.dw = GET_REG(pinctrl_obj.vbase + i * 4);
            reg.bit.oe = 0;
            SET_REG(pinctrl_obj.vbase + i * 4, reg.dw);
            continue;
        }
        pin->reg->dw = GET_REG(pinctrl_obj.vbase +
                pin->reg_offset);

        pin->input_enable = pin->reg->bit.ie;
        pin->output_enable = pin->reg->bit.oe;
    }
}

void fh_pinctrl_init(unsigned int base)
{
    int boot_mode = GET_REG(REG_PMU_BOOT_MODE);

    pinctrl_obj.vbase = pinctrl_obj.pbase = (void *)base;

    fh_pinctrl_init_devicelist(&fh_pinctrl_devices);
    fh_pinctrl_init_devices(fh_pinctrl_selected_devices,
            ARRAY_SIZE(fh_pinctrl_selected_devices), 0);
    fh_pinctrl_init_pin();

    /* REG_PMU_BOOT_MODE=4, boot from emmc */
    if (boot_mode == 4)
        fh_pinctrl_sdev("SD1_EMMC", 0);
}

void fh_pinctrl_prt(void)
{
    int i;
    printf("%2s\t%8s\t%4s\t%8s\t%4s\t%4s\t%4s\t%4s\n",
		    "id", "name", "addr", "reg", "sel", "ie", "oe", "pupd");
    for(i=0; i<PAD_NUM; i++)
    {
        if(!pinctrl_obj.pinlist[i])
        {
                OS_PRINT("ERROR: pad %d is empty\n", i);
		continue;
        }
        printf("%02d\t%8s\t0x%04x\t0x%08x\t%04d\t%04d\t%04d\t%04d\n",
                pinctrl_obj.pinlist[i]->pad_id,
                pinctrl_obj.pinlist[i]->func_name,
                pinctrl_obj.pinlist[i]->reg_offset + (unsigned int)pinctrl_obj.vbase,
                GET_REG(pinctrl_obj.vbase + pinctrl_obj.pinlist[i]->reg_offset),
                pinctrl_obj.pinlist[i]->func_sel,
                pinctrl_obj.pinlist[i]->input_enable,
                pinctrl_obj.pinlist[i]->output_enable,
                pinctrl_obj.pinlist[i]->pullup_pulldown);
    }

}


int fh_pinctrl_smux(char *devname, char* muxname, int muxsel, unsigned int flag)
{
    PinCtrl_Device *dev;
    int i, ret;
    PinCtrl_Mux *foundmux = NULL;
    char *oldfunc = NULL;

    if (flag & PIN_RESTORE) {
        foundmux = fh_pinctrl_get_mux_by_name(muxname);
        if (foundmux == NULL) {
            OS_PRINT("ERROR: PIN_RESTORE, cannot found mux: %s\n",
                muxname);
            return -10;
        }
        goto mux_switch;
    }

    dev = fh_pinctrl_get_device_by_name(devname);

    if(!dev)
    {
        OS_PRINT("ERROR: cannot find device %s\n", devname);
        return -4;
    }

    for(i=0; i<dev->mux_count; i++)
    {
        unsigned int *mux_addr = (unsigned int *)((unsigned int)dev
                + sizeof(*dev) - 4 + i*4);
        PinCtrl_Mux *mux = (PinCtrl_Mux *)(*mux_addr);

        if (!strcmp(muxname, mux->mux_pin[0]->func_name)) {
            foundmux = mux;
            mux->cur_pin = muxsel;
            goto mux_switch;
        }
    }

    if(i == dev->mux_count)
    {
        OS_PRINT("ERROR: cannot find mux %s of device %s\n", muxname, devname);
        return -6;
    }

mux_switch:
    if (flag & PIN_BACKUP) {
        int id = foundmux->mux_pin[foundmux->cur_pin]->pad_id;
        PinCtrl_Pin *pin = pinctrl_obj.pinlist[id];

        if (pin == NULL) {
            OS_PRINT("ERROR: PIN_BACKUP, oldpin is null\n");
            return 0;
        }
        oldfunc = pin->func_name;
    }
    ret = fh_pinctrl_mux_switch(foundmux, flag);
    if (flag & PIN_BACKUP)
        ret = (int)oldfunc;

    if(flag & NEED_CHECK_PINLIST)
        fh_pinctrl_check_pinlist();

    return ret;
}

int fh_pinctrl_sdev(char *devname, unsigned int flag)
{
    PinCtrl_Device *dev;
    int ret, i;
    int print = !(flag & NO_PRINT);

    dev = fh_pinctrl_get_device_by_name(devname);
    if(!dev)
    {
        OS_PRINT("ERROR: cannot find device %s\n", devname);
        return -7;
    }

    if (print)
        OS_PRINT("%s:\n\t", dev->dev_name);
    ret = fh_pinctrl_device_switch(dev, flag);

    if(ret)
    {
        OS_PRINT("%s: %s failed\n", __func__, devname);
        return ret;
    }

    for(i = 0; i < dev->mux_count; i++)
    {
        unsigned int *mux_addr = (unsigned int *)((unsigned int)dev
                                 + sizeof(*dev) - 4 + i * 4);
        PinCtrl_Mux *mux = (PinCtrl_Mux *)(*mux_addr);

        if (print) {
            OS_PRINT(mux->mux_pin[0]->func_name);
            OS_PRINT(" ");
        }
    }
    if (print)
        OS_PRINT("\n\n");

    if(flag & NEED_CHECK_PINLIST)
        fh_pinctrl_check_pinlist();

    return 0;
}

int fh_pinctrl_set_oe(char *pin_name, unsigned int oe)
{
    PinCtrl_Pin *pin = NULL;

    if (oe > 1) {
        OS_PRINT("oe is 0 or 1\n");
        return -1;
    }

    pin = fh_pinctrl_get_pin_by_name(pin_name);

    if (!pin) {
        OS_PRINT("ERROR: cannot find pin %s\n", pin_name);
        return -2;
    }

    pin->output_enable = oe;
    return fh_pinctrl_func_select(pin, 0);
}

char *fh_pinctrl_smux_backup(char *devname, char *muxname, int muxsel){
    return (char*)fh_pinctrl_smux(devname, muxname, muxsel, PIN_BACKUP);
}

char *fh_pinctrl_smux_restore(char *devname, char *muxname, int muxsel){
    return (char*)fh_pinctrl_smux(devname, muxname, muxsel, PIN_RESTORE);
}

static int do_pinctrl_smux(int argc, char *argv[])
{
	char *endp;
	int ret;
	int sel;

	if (argc < 4)
		goto usage;

	sel = simple_strtoul(argv[3], &endp, 16);
	if (*argv[3] == 0 || *endp != 0)
		goto usage;

	ret = fh_pinctrl_smux(argv[1], argv[2], sel, 0);
	if (ret) {
		printf("pinctrl %s failed\n", argv[0]);
		return 1;
	}

	return 0;

usage:
	puts("Usage: pinctrl smux devname muxname muxid\n");
	return 1;
}

static int do_pinctrl_sdev(int argc, char *argv[])
{
	int ret;

	if (argc < 1)
		goto usage;

	ret = fh_pinctrl_sdev(argv[1], 0);
	if (ret) {
		printf("pinctrl %s failed\n", argv[0]);
		return 1;
	}

	return 0;

usage:
	puts("Usage: pinctrl print devname\n");
	return 1;
}


static int do_pinctrl(cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
{
	const char *cmd;
	/* need at least two arguments */
	if (argc < 2)
		goto usage;

	cmd = argv[1];

	if (strcmp(cmd, "printall") == 0)
	{
		fh_pinctrl_prt();
		return 0;
	}

	if (strcmp(cmd, "sdev") == 0)
		return do_pinctrl_sdev(argc - 1, argv + 1);

	if (strcmp(cmd, "smux") == 0)
		return do_pinctrl_smux(argc - 1, argv + 1);

usage:
	cmd_usage(cmdtp);
	return 1;
}

#include <command.h>

U_BOOT_CMD(
	pinctrl,	5,	1,	do_pinctrl,
	"Pin Ctrl",
	"printall                              - print all the current pinctrl settings\n"
	"	sdev devname                          - switch one specific pinctrl setting\n"
	"	smux devname muxname muxid            - switch pinmux\n\n"
);
